import logging
import time

from django.db import models
from numpy import *  # 导入numpy的库函数

# Create your models here.
from .drive.dc_motor import DcMotor

LOG = logging.getLogger()
logging.basicConfig(level=logging.INFO)


class ControlCar:
    ControlObject = DcMotor()
    CarRadius = 10

    @classmethod
    def calc_3_speed(cls, x, y, theta):
        car_matrix = mat([x, y, theta]).T
        change_matrix = mat([
            [-sin(theta), cos(theta), cls.CarRadius],
            [-sin(pi / 3 - theta), -cos(pi / 3 - theta), cls.CarRadius],
            [sin(pi / 3 + theta), -cos(pi / 3 + theta), cls.CarRadius]
        ])
        result = (change_matrix * car_matrix)
        result = result.tolist()
        result = [x for j in result for x in j]
        LOG.info('calc_3_speed is {}'.format(result))
        return result

    @classmethod
    def control_car(cls, x, y):
        if x == 0 and y == 0:
            cls.ControlObject.set_3_dir([None, None, None])
            return
        x, y = 25 * x, 25 * y
        theta = arctan(y / x) if x != 0 else pi / 2
        speed = cls.calc_3_speed(x * cos(theta), y * sin(theta), theta)
        dir_list = [True if i >= 0 else False for i in speed]
        speed_list = [abs(i) for i in speed]
        LOG.info('send dirs={}, speed={}'.format(dir_list, speed_list))
        cls.ControlObject.set_3_dir(dir_list)
        cls.ControlObject.set_3_dc(speed_list)


if __name__ == '__main__':
    ControlCar.control_car(2, 1)
    time.sleep(5)
    ControlCar.control_car(1, 2)
    time.sleep(5)
